/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-06-16     Administrator       the first version
 */

#include "app_pid.h"


/**********************速度环*************************/
//四个电机的速度环参数结构体
Motor_parameter MOTOR_ve[4];
//轮子总体目标速度
static float A_Ve_Target, B_Ve_Target, C_Ve_Target, D_Ve_Target = 0.0f;
static float A_PWM = 0.0f, B_PWM = 0.0f, C_PWM = 0.0f, D_PWM = 0.0f;
rt_int32_t PWM_cirle = 0;


/**********************编码器位置环*************************/
//四个电机的位置环(编码器)参数结构体
Motor_parameter MOTOR_po[4];
//电机的目标位置
//static rt_int64_t A_Po_Target, B_Po_Target, C_Po_Target, D_Po_Target = 0;
//电机的实际位置
//static rt_int32_t A_Po_Measure, B_Po_Measure, C_Po_Measure, D_Po_Measure;
//编码器的位置环输出，是要串进速度环里计算的
//static float A_po_target_speed, B_po_target_speed, C_po_target_speed, D_po_target_speed = 0.0f;

/**********************陀螺仪角度环，用来自旋的*************************/
//四个电机的角度环参数结构体
Motor_parameter MOTOR_Ag[4];
//角度环的目标位置
static float A_Ag_Target, B_Ag_Target, C_Ag_Target, D_Ag_Target = 0.0f;
//角度环的实际位置
static float A_Ag_Measure, B_Ag_Measure, C_Ag_Measure, D_Ag_Measure;
//角度环输出，是要串进速度环里计算的（普通四轮小车），麦轮不用，它是单独一个旋转轴。
static float A_Ag_target_speed, B_Ag_target_speed, C_Ag_target_speed, D_Ag_target_speed = 0.0f;

/**********************摄像头位置环,嵌赛关键环，利用它来对准玩具，中心点（400，240）*************************/
//四个电机的位置环(摄像头找物体)参数结构体
Motor_parameter MOTOR_k230_x[4];
Motor_parameter MOTOR_k230_d[4];
Motor_parameter MOTOR_k230_catch_x[4];
Motor_parameter MOTOR_k230_catch_y[4];
//摄像头位置环的速度修正输出量
float A_K230_target_xx, B_K230_target_xx, C_K230_target_xx, D_K230_target_xx = 0.0f;//x中心坐标，麦轮车y轴量
float A_K230_target_distance, B_K230_target_distance, C_K230_target_distance, D_K230_target_distance = 0.0f;//距离，麦轮车x轴量
float A_K230_target_catch_x, B_K230_target_catch_x, C_K230_target_catch_x, D_K230_target_catch_x = 0.0f;//机械臂抓的x坐标控制左右移动输出，麦轮车的Y轴量
float A_K230_target_catch_y, B_K230_target_catch_y, C_K230_target_catch_y, D_K230_target_catch_y = 0.0f;//机械臂抓的y坐标控制前后移动输出，麦轮车的X轴量


/*-------------------------- 控制功能使能标志位 --------------------------*/
__attribute__((used)) static char K230_position_pid_enable_flag = 0;   ///< 摄像头位置环使能标志
__attribute__((used)) static char angle_pid_enable_flag = 0;  ///< 角度环使能标志
__attribute__((used)) static char encoder_position_pid_enable_flag = 0;   ///< 编码器位置环使能标志
__attribute__((used)) static char Mai_run_enable_flag = 0;   ///< 麦轮全向运动环使能标志
__attribute__((used)) static char K230_catch_pid_enable_flag = 0;   ///< 摄像头抓取环使能标志
__attribute__((used)) static char activate_pid_sudo_flag = 1;//速度环使能
/************************基准速度******************************/
rt_int32_t base_speed = 0;//单位是编码器值-90到90


/*使用static可以隐藏细节，其它源文件无法调用，通过公共接口对外暴露功能*/
// PID参数初始化接口
static void positional_pid_params_init(Motor_parameter *positional_pid,
                                  float _kp, float _ki, float _kd,
                                  float _dead_zone, float _output_max,
                                  float _output_min)
{
    // 初始化 PID 参数
    positional_pid->Velocity_KP = _kp;  // 比例增益
    positional_pid->Velocity_KI = _ki;  // 积分增益
    positional_pid->Velocity_KD = _kd;  // 微分增益

    // 初始化死区、输出上限和输出下限
    positional_pid->dead_zone = _dead_zone;  // 死区范围
    positional_pid->output_max = _output_max;  // 输出最大值
    positional_pid->output_min = _output_min;  // 输出最小值

    // 初始化目标值和输出值
    positional_pid->Target = 0;  // 目标值初始化为0
    positional_pid->output = 0;  // 输出值初始化为0
}

// PID动态参数修改接口
static void positional_pid_set_value(Motor_parameter *positional_pid,
                                     float _kp, float _ki, float _kd,
                                     float _separation_threshold,
                                     float _integral_limit_max,
                                     float _integral_limit_min)
{
    // 设置 PID 参数
    positional_pid->Velocity_KP = _kp;  // 比例增益
    positional_pid->Velocity_KI = _ki;  // 积分增益
    positional_pid->Velocity_KD = _kd;  // 微分增益

    positional_pid->separation_threshold = _separation_threshold;  // 积分分离阈值
    positional_pid->integral_limit_max = _integral_limit_max;  // 积分限幅最大值
    positional_pid->integral_limit_min = _integral_limit_min;  // 积分限幅最小值
}

/* 控制PID的使能状态 ，这个没有使用static的功能*/
static void positional_pid_control(Motor_parameter *positional_pid, positional_pid_status _status)
{
    positional_pid->control = _status;  // 设置PID控制器的使能状态
}

/*PID控制，位置式*/
/*参数填写每个电机的控制结构体*/
float Moter_pid_compute(Motor_parameter *Moter_pid, float _target, rt_int32_t _measure)
{
    if(Moter_pid->control == PID_ENABLE)
    {
        // 设置目标值和测量值
        Moter_pid->Target = _target;  // 设置目标值
        Moter_pid->Measure = (float)_measure;  // 设置测量值

        // 更新上次误差
        Moter_pid->last_error = Moter_pid->error;

        // 获取当前误差
        Moter_pid->error = Moter_pid->Target - Moter_pid->Measure;

        //输入死区
        if(fabs(Moter_pid->error) < Moter_pid-> dead_zone)
        {
            Moter_pid->output = 0.0f;
        }
        else {
            //误差积分（累加）
            Moter_pid->error_Int += Moter_pid->error;

            /*误差积分+积分分离*/
//            if(fabs(Moter_pid->error) < 50)
//            {
//                Moter_pid->error_Int += Moter_pid->error;  //正常积分
//            }
//            else {
//                Moter_pid->error_Int = 0;//此时不进行积分，同时把误差积分直接清零
//            }

            //积分限幅，最大和最小值通过用output的输出/ki的系数确定
            if (Moter_pid->error_Int > Moter_pid->integral_limit_max)
            {
                Moter_pid->error_Int = Moter_pid->integral_limit_max;
            }
            if (Moter_pid->error_Int < Moter_pid->integral_limit_min)
            {
                Moter_pid->error_Int = Moter_pid->integral_limit_min;
            }

            // 计算比例项
            Moter_pid->p_out = Moter_pid->Velocity_KP * Moter_pid->error;
            //计算积分项
            Moter_pid->i_out = Moter_pid->Velocity_KI * Moter_pid->error_Int;
            // 计算微分项
            Moter_pid->d_out = Moter_pid->Velocity_KD * (Moter_pid->error - Moter_pid->last_error);
            // 计算总输出
            Moter_pid->output = Moter_pid->p_out + Moter_pid->i_out + Moter_pid->d_out;

            //限幅处理
            // 限制输出在输出上限和输出下限之间
            if (Moter_pid->output > Moter_pid->output_max)
            {
                Moter_pid->output = Moter_pid->output_max;  // 输出超过最大值，设为最大值
            }
            if (Moter_pid->output < (Moter_pid->output_min))
            {
                Moter_pid->output = Moter_pid->output_min;  // 输出低于最小值，设为最小值
            }

            /*输出偏移,这个偏移值是使得车刚好转动的值*/
//            if (Moter_pid->output > 0.0)        //如果输出为正
//            {
//                Moter_pid->output += 6.0f;       //则直接给输出值加上一个固定偏移
//            }
//            else if (Moter_pid->output < 0.0)   //如果输出为负
//            {
//                Moter_pid->output -= 6.0f;       //则直接给输出值减去一个固定偏移
//            }
//            else                //输出为0
//            {
//                Moter_pid->output = 0;        //输出0
//            }

            return Moter_pid->output;  // 返回计算后的PID输出值
        }
    }
    else {
        return 0.0f;
    }
}


/*PID控制，速度式*/
/*参数填写每个电机的控制结构体*/
float Moter_pid_compute_V(Motor_parameter *Moter_pid, float _target, float _measure)
{
    if (Moter_pid->control == PID_ENABLE)
    {
        // 设置目标值和测量值
        Moter_pid->Target = _target;
        Moter_pid->Measure = _measure;

        // 更新误差历史
        Moter_pid->prev_error = Moter_pid->last_error;//获取上上次的误差
        Moter_pid->last_error = Moter_pid->error;//获取上次的误差
        // 当前误差
        Moter_pid->error = Moter_pid->Target - Moter_pid->Measure;

        // 比例项：当前误差与上一次误差之差
        Moter_pid->p_out = Moter_pid->Velocity_KP * (Moter_pid->error - Moter_pid->last_error);

        //积分项：当前误差
        Moter_pid->i_out = Moter_pid->Velocity_KI * Moter_pid->error;

        //积分限幅，最大和最小值通过用output的输出/ki的系数确定
        if (Moter_pid->error_Int > Moter_pid->integral_limit_max)
        {
            Moter_pid->error_Int = Moter_pid->integral_limit_max;
        }
        if (Moter_pid->error_Int < Moter_pid->integral_limit_min)
        {
            Moter_pid->error_Int = Moter_pid->integral_limit_min;
        }

        // 微分项：e(t) - 2e(t-1) + e(t-2)
        Moter_pid->d_out = Moter_pid->Velocity_KD *
                           (Moter_pid->error - 2 * Moter_pid->last_error + Moter_pid->prev_error);

        // 输出增量
        float delta_output = Moter_pid->p_out + Moter_pid->i_out + Moter_pid->d_out;

        // 叠加输出增量到总输出
        Moter_pid->output += delta_output;

        // 限幅
        if (Moter_pid->output > Moter_pid->output_max)
            Moter_pid->output = Moter_pid->output_max;
        if (Moter_pid->output < Moter_pid->output_min)
            Moter_pid->output = Moter_pid->output_min;

        return Moter_pid->output;
    }
    else
    {
        return 0.0f;
    }
}

/*手动清零 PID 控制器状态*/
void pid_reset(Motor_parameter *pid)
{
    pid->output = 0;
    pid->p_out = 0;
    pid->i_out = 0;
    pid->d_out = 0;
    pid->error = 0;
    pid->last_error = 0;
    pid->prev_error = 0;
    pid->error_Int = 0;
}


/* 初始化位置式PID控制器参数 */
void positional_pid_init(Motor_parameter *positional_pid, float kp,
                         float ki, float kd, float dead_zone, float output_max,
                         float output_min)
{
    // 初始化 PID 控制器
      /*在这里暴露函数的接口*/
      /*在C语言中，​函数名本身是一个指向该函数的指针常量​（即函数地址）。
            因此，当你在代码中写 positional_pid_params_init 时，它等价于 &positional_pid_params_init（取函数地址），两者可以互换使用*/
      /*positional_pid_params_init 是结构体中的一个函数指针成员，它是一个变量，即一个名字，用于存储某个函数的地址*/
      /*即一个指向接受特定参数并返回 void 的函数的指针*/
      /*该指针的类型是：void (*)(struct _positional_pid_params_t*, float, float, float, float, float, float);*/
    positional_pid->positional_pid_params_init = positional_pid_params_init;  // 设置参数初始化函数接口
    positional_pid->positional_pid_set_value = positional_pid_set_value;  // 设置动态参数修改函数接口
    positional_pid->positional_pid_control = positional_pid_control;  // 设置PID使能控制函数接口

    positional_pid->positional_pid_params_init(positional_pid, kp, ki, kd, dead_zone, output_max, output_min);// 调用初始化PID参数函数接口

    positional_pid->control = PID_ENABLE;  // 启用PID控制，默认使能 PID 控制器
}

/*设置基准速度*/
void set_base_speed(rt_int32_t _speed)
{
    base_speed = _speed;          // 设置基础速度
//    if (_speed <= 0.1f)                // 极低速判断（接近停止）
//    {
//        Drive_motor(0,0,0,0);       // 直接关闭电机1输出
//    }
}

// 激活车四个电机摄像头机械臂抓取PID
void active_K230_catch_pid(void) { K230_catch_pid_enable_flag = 1; }
// 停用四个电机摄像头机械臂抓取PID
void deactive_K230_catch_pid(void) { K230_catch_pid_enable_flag = 0; }

// 激活车四个电机摄像头位置PID
void active_K230_position_pid(void) { K230_position_pid_enable_flag = 1; }
// 停用四个电机摄像头位置PID
void deactive_K230_position_pid(void) { K230_position_pid_enable_flag = 0; }

// 激活车角度环PID
void active_angle_pid(void) { angle_pid_enable_flag = 1; }
// 停用车角度环PID
void deactive_angle_pid(void) { angle_pid_enable_flag = 0; }

// 激活车四个电机编码器位置PID
void active_encoder_position_pid(void) { encoder_position_pid_enable_flag = 1; }
// 停用四个电机编码器位置PID
void deactive_encoder_position_pid(void) { encoder_position_pid_enable_flag = 0; }

// 激活车四个电机的麦轮全向运动PID
void active_mai_run_pid(void) { Mai_run_enable_flag = 1; }
// 停用四个电机的麦轮全向运动PID
void deactive_mai_run_pid(void) { Mai_run_enable_flag = 0; }

// 激活速度环
void activate_PID_SUDO(void) { activate_pid_sudo_flag = 1; }
// 停用四个电机的麦轮全向运动PID
void deactivate_PID_SUDO(void) { activate_pid_sudo_flag = 0; }



//rt_int16_t toy_distance = 30;// 离玩具的距离，接收k230发过来的值
//rt_int16_t toy_x = 400;// 玩具的x位置，接收k230发过来的值
rt_int16_t LCD_y = 240, LCD_x = 400;//屏幕中玩具的坐标
rt_int16_t dist = 25;

/********************交互调试*******************************/
void change_distance_60(void)  {toy_distance = 60;}
void change_distance_40(void)  {toy_distance = 40;}
void change_distance_30(void)  {toy_distance = 30;}
void change_distance_50(void)  {toy_distance = 50;}
void change_distance_20(void)  {toy_distance = 20;}

void change_x_600(void)  {toy_x = 600;}
void change_x_400(void)  {toy_x = 400;}
void change_x_300(void)  {toy_x = 300;}
void change_x_500(void)  {toy_x = 500;}
void A_50_Ve_Target(void) {A_Ve_Target = 50;}
void A_40_Ve_Target(void) {A_Ve_Target = 40;}
void A_60_Ve_Target(void) {A_Ve_Target = 60;}
void A_80_Ve_Target(void) {A_Ve_Target = 80;}
void A_70_Ve_Target(void) {A_Ve_Target = 70;}
/********************交互调试*******************************/

/*PID计算的任务函数*/
/*这里用的串级PID，所有环的输出都作为速度环的输入，从而得到一个稳定的速度*/
void Pid_task(void *parameter)
{
    rt_tick_t  last_wake_time = rt_tick_get(); //获取上次的值

    /*陀螺仪*/
    static uint8_t data_index = 0;
    static uint8_t rx_state = WAIT_HEADER1;

    app_pid_status_t app_pid_status = APP_PID_INIT; //当前PID控制状态

    float Car_HW=(Robot_Parament.WheelSpacing + Robot_Parament.AxleSpacing); // 已经是一半了

//    rt_int16_t A_PWM = 0;
//    rt_int16_t B_PWM = 0;
//    rt_int16_t C_PWM = 0;
//    rt_int16_t D_PWM = 0;//电机的PWM输出值


//    rt_int16_t toy_x = 0;// 玩具的x位置，接收k230发过来的值
//    rt_int16_t toy_distance = 0;// 离玩具的距离，接收k230发过来的值
//      50000 / 90 = 556
//    A_Ve_Target = 60.0f;
//    B_Ve_Target = 60.0f;
//    C_Ve_Target = 60.0f;
//    D_Ve_Target = 60.0f;

    while(1)
    {
        rt_thread_delay_until(&last_wake_time, 10);

            /*陀螺仪*/
            uint8_t temp = 0;

            while (rt_ringbuffer_getchar(&gyro_rx_rb, &temp) == 1)
            {
                switch (rx_state)
                {
                case WAIT_HEADER1:
                    if (temp == 0x55)
                        rx_state = WAIT_HEADER2;
                    break;
                case WAIT_HEADER2:
                    rx_state = (temp == 0x53) ? RECEIVE_DATA : WAIT_HEADER1;
                    data_index = 0;
                    break;
                case RECEIVE_DATA:
                    receivedData[data_index++] = temp;
                    if (data_index >= 9)
                    {
                        process_euler_angles();
                        rx_state = WAIT_HEADER1;
                    }
                    break;
                }
            }
            switch (app_pid_status) {
                case APP_PID_INIT:
                    /****************************速度环********************************/
                    // 四个电机速度环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_ve[i], 0, 0, 0, 0.0f, 50000, -50000);
                    }
                    //设置速度环参数,四个轮子都是一样的，写个for循环吧
//                    for(int i = 0; i < 4; i++)
//                    {
                       MOTOR_ve[A].positional_pid_set_value(&MOTOR_ve[A], 3500, 100, 0, 0, 500, -500);//p  1.5 i 0.1 d 0 积分分离阈值 0  积分限幅 0
                       MOTOR_ve[B].positional_pid_set_value(&MOTOR_ve[B], 3500, 100, 0, 0, 500, -500);//pid 积分分离阈值 5 积分限幅 5和-5
                       MOTOR_ve[C].positional_pid_set_value(&MOTOR_ve[C], 3500, 100, 0, 0, 500, -500);//pid 积分分离阈值 5 积分限幅 5和-5
                       MOTOR_ve[D].positional_pid_set_value(&MOTOR_ve[D], 3500, 100, 0, 0, 500, -500);//pid 积分分离阈值 5 积分限幅 5和-5
//                    }
                    /******************************角度环********************************/
                    // 四个电机角度环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_Ag[i], 0, 0, 0, 0.0f, 80, -80);
                    }
                    //设置角度环参数,四个轮子都是一样的，写个for循环吧
//                    for(int i = 0; i < 4; i++)
//                    {
                        MOTOR_Ag[A].positional_pid_set_value(&MOTOR_Ag[A], 3.3, 0, 2, 0, 0, 0);//3.0, 0, 0.5
                        MOTOR_Ag[B].positional_pid_set_value(&MOTOR_Ag[B], 3.3, 0, 2, 0, 0, 0);//3.0, 0, 0.5
                        MOTOR_Ag[C].positional_pid_set_value(&MOTOR_Ag[C], 3.3, 0, 2, 0, 0, 0);//6.5, 0, 0.8
                        MOTOR_Ag[D].positional_pid_set_value(&MOTOR_Ag[D], 3.3, 0, 2, 0, 0, 0);//6.5, 0, 0.8
//                    }
                    /******************************摄像头x坐标位置环**************************/
                    // 四个电机摄像头位置环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_k230_x[i], 0, 0, 0, 0, 20, -20);//pid 死区 输出最大值 输出最小值
                    }
                    //设置摄像头环参数,四个轮子都是一样的，写个for循环吧
                    for(int i = 0; i < 4; i++)
                    {
                        MOTOR_k230_x[i].positional_pid_set_value(&MOTOR_k230_x[i], -0.1, 0.0, 0.0, 0 ,0 ,0);//pid 积分分离阈值 5 积分限幅 5和-5
                    }
                    /******************************摄像头distance位置环**************************/
                    // 四个电机摄像头位置环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_k230_d[i], 0, 0, 0, 0.0f, 30, -30);//pid 死区 输出最大值 输出最小值
                    }
                    //设置摄像头环参数,四个轮子都是一样的，写个for循环吧
                    for(int i = 0; i < 4; i++)
                    {
                        MOTOR_k230_d[i].positional_pid_set_value(&MOTOR_k230_d[i], -1.0, 0, 0.0,0 ,0 , 0);//pid 积分分离阈值 5 积分限幅 5和-5
                    }
                    /******************************摄像头抓取x坐标左右**************************/
                    // 四个电机摄像头位置环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_k230_catch_x[i], 0, 0, 0, 0.0f, 20, -20);//pid 死区 输出最大值 输出最小值
                    }
                    //设置摄像头环参数,四个轮子都是一样的，写个for循环吧
                    for(int i = 0; i < 4; i++)
                    {
                        MOTOR_k230_catch_x[i].positional_pid_set_value(&MOTOR_k230_catch_x[i], -0.1, 0, 0, 0 ,0 , 0);//pid 积分分离阈值 5 积分限幅 5和-5
                    }
                    /******************************摄像头抓取y坐标前后**************************/
                    // 四个电机摄像头位置环PID初始化
                    for(int i = 0; i < 4; i++)
                    {
                        positional_pid_init(&MOTOR_k230_catch_y[i], 0, 0, 0, 0.0f, 20, -20);//pid 死区 输出最大值 输出最小值
                    }
                    //设置摄像头环参数,四个轮子都是一样的，写个for循环吧
                    for(int i = 0; i < 4; i++)
                    {
                        MOTOR_k230_catch_y[i].positional_pid_set_value(&MOTOR_k230_catch_y[i], 0.1, 0, 0,0 ,0 , 0);//pid 积分分离阈值 5 积分限幅 5和-5
                    }


                    app_pid_status++;  // 切换到运行状态
                    break;
                case  APP_PID_RUN:

                    /*读取编码器的数据*/
                    ReadEncoder();

                    //计算四个电机速度环的PWM值，首次进来设置PWM是动不起来的，因为速度目标值是0，编码器的值直接就是它根据周期计算得到的速度

                    //四轮小车
                    //四个电机速度环计算PWM值（目标速度 vs 实际速度），这个速度环只是为了得到稳定的速度
//                    A_PWM = (int16_t)Moter_pid_compute(&MOTOR_ve[A], A_Ve_Target, get_encoderA_speed());//A_Ve_Target这个目标速度是在下面合成的
//                    B_PWM = (int16_t)Moter_pid_compute(&MOTOR_ve[B], B_Ve_Target, get_encoderB_speed());
//                    C_PWM = (int16_t)Moter_pid_compute(&MOTOR_ve[C], C_Ve_Target, get_encoderC_speed());
//                    D_PWM = (int16_t)Moter_pid_compute(&MOTOR_ve[D], D_Ve_Target, get_encoderD_speed());
//                    Drive_motor(A_PWM, B_PWM, C_PWM, D_PWM);//给四个电机赋值PWM

                    if(activate_pid_sudo_flag == 1)
                    {
                        //麦轮，这个速度环只是为了得到稳定的速度，串级
                        A_PWM = Moter_pid_compute(&MOTOR_ve[A], A_Ve_Target, -A_encoder.Encoder);
                        B_PWM = Moter_pid_compute(&MOTOR_ve[B], B_Ve_Target, -B_encoder.Encoder);
                        C_PWM = Moter_pid_compute(&MOTOR_ve[C], C_Ve_Target, C_encoder.Encoder);
                        D_PWM = Moter_pid_compute(&MOTOR_ve[D], D_Ve_Target, D_encoder.Encoder);
                        Drive_motor((rt_int32_t)A_PWM, (rt_int32_t)B_PWM, (rt_int32_t)C_PWM, (rt_int32_t)D_PWM);
                    }
//                    else {
//                        A_PWM = 0;
//                        B_PWM = 0;
//                        C_PWM = 0;
//                        D_PWM = 0;
//                        for(int i = 0; i < 4; i++)
//                         {
//                            pid_reset(&MOTOR_ve[i]);
//                         }
//                    }
                    rt_kprintf("%d,%d\r\n",dist,(rt_int16_t)toy_distance);
//                    rt_kprintf("%d,%d,%d,%d,%d\r\n", (rt_int32_t)A_Ve_Target,-(rt_int32_t)A_encoder.Encoder,-(rt_int32_t)B_encoder.Encoder,(rt_int32_t)C_encoder.Encoder,(rt_int32_t)D_encoder.Encoder);
//                    Drive_motor((rt_int32_t)A_PWM, (rt_int32_t)B_PWM, (rt_int32_t)C_PWM, (rt_int32_t)D_PWM);
//                    rt_kprintf("A_PWM : %d\r\n", (rt_int32_t)A_PWM);
//                    rt_kprintf("B_PWM : %d\r\n", (rt_int32_t)B_PWM);
//                    rt_kprintf("C_PWM : %d\r\n", (rt_int32_t)C_PWM);
//                    rt_kprintf("D_PWM : %d\r\n", (rt_int32_t)D_PWM);
//                    rt_kprintf("A_e : %d\r\n", (rt_int32_t)MOTOR_ve[A].error);
//                    rt_kprintf("KP=%d, KI=%d, KD=%d\n",
//                               (int16_t)(MOTOR_ve[A].Velocity_KP * 100),
//                               (int16_t)(MOTOR_ve[A].Velocity_KI * 100),
//                               (int16_t)(MOTOR_ve[A].Velocity_KD)* 100);
//                      rt_kprintf("A_e : %d\r\n", (rt_int32_t)(MOTOR_ve[A].error * 100));
//                    rt_kprintf("B_e : %d\r\n", (rt_int32_t)MOTOR_ve[B].error);
//                    rt_kprintf("C_e : %d\r\n", (rt_int32_t)MOTOR_ve[C].error);
//                    rt_kprintf("D_e : %d\r\n", (rt_int32_t)MOTOR_ve[D].error);
//                            rt_kprintf("[EncoderTimer] Tick: %u | Encoder A: %d B: %d C: %d D: %d\n",
//                                       rt_tick_get(),
//                                       -(rt_int32_t)A_encoder.Encoder,
//                                       -(rt_int32_t)B_encoder.Encoder,
//                                       (rt_int32_t)C_encoder.Encoder,
//                                       (rt_int32_t)D_encoder.Encoder);
//                                            rt_kprintf("Yaw : %d \r\n", (int16_t)(Yaw*1000));

                    //获取编码器的值，位置环用的是总值
                    //判断位置环是否使能，若有则计算位置环输出的速度目标值，记得正负号
                    //未使能则同步一下目标位置，把测量值给目标值，位置环输出的速度目标值归零

                    // 获取编码器位置（校准值减去30000）
//                    A_Po_Measure = get_A_total_count() - 30000;
//                    B_Po_Measure = get_B_total_count() - 30000;
//                    C_Po_Measure = get_C_total_count() - 30000;
//                    D_Po_Measure = get_D_total_count() - 30000;
//
//                    // 编码器位置环使能判断
//                    if (encoder_position_pid_enable_flag == 1)
//                    {
//                        // 计算位置环输出的速度目标（转换为速度环输入）
//                        A_po_target_speed = Moter_pid_compute(
//                            &MOTOR_po[A], (float)A_Po_Target,
//                            (float)A_Po_Measure);                      //目标值已给接口，在下面注释掉了
//                        // 电机2速度符号反向处理（差速控制）
//                        B_po_target_speed = -(Moter_pid_compute(
//                            &MOTOR_po[B], (float)B_Po_Target,
//                            (float)B_Po_Measure));
//                        C_po_target_speed = Moter_pid_compute(
//                            &MOTOR_po[C], (float)C_Po_Target,
//                            (float)C_Po_Measure);
//                        // 电机2速度符号反向处理（差速控制）
//                        D_po_target_speed = -(Moter_pid_compute(
//                            &MOTOR_po[D], (float)D_Po_Target,
//                            (float)D_Po_Measure));
//                    }
//                    else
//                    {
//                        // 位置环禁用时同步目标位置
//                        A_Po_Target = A_Po_Measure;
//                        B_Po_Target = B_Po_Measure;
//                        C_Po_Target = C_Po_Measure;
//                        D_Po_Target = D_Po_Measure;
//                        A_po_target_speed = 0;
//                        B_po_target_speed = 0;
//                        C_po_target_speed = 0;
//                        D_po_target_speed = 0;
//                    }

                    //判断角度环是否使能，有则计算角度环输出的速度目标值
                    //未使能则把角度环输出的速度目标值归零
                    //速度目标合成（基础速度+位置修正+角度修正+巡线修正），在这里得到一个真正的驱动的值，且基础速度不能为0，否则动不了
                    // 角度环使能判断
                    //角度往右边是负，往左边是正
                    if (angle_pid_enable_flag == 1)
                    {
                        A_Ag_Measure = Yaw;
                        B_Ag_Measure = Yaw;
                        C_Ag_Measure = Yaw;
                        D_Ag_Measure = Yaw;
                        A_Ag_target_speed = -Moter_pid_compute(
                            &MOTOR_Ag[A], (float)A_Ag_Target,
                            (float)A_Ag_Measure);
                        B_Ag_target_speed = -Moter_pid_compute(
                            &MOTOR_Ag[B], (float)B_Ag_Target,
                            (float)B_Ag_Measure);
                        C_Ag_target_speed = -Moter_pid_compute(
                            &MOTOR_Ag[C], (float)C_Ag_Target,
                            (float)C_Ag_Measure);
                        D_Ag_target_speed = -Moter_pid_compute(
                            &MOTOR_Ag[D], (float)D_Ag_Target,
                            (float)D_Ag_Measure);
//                        rt_kprintf("%d,%d\r\n", (int16_t)A_Ag_Target,(int16_t)(Yaw));
//                        rt_kprintf("%d,%d,%d,%d,%d\r\n", );
//                        rt_kprintf("A_Ag_target_speed :%d\r\n", (rt_int32_t)A_Ag_target_speed);
//                        rt_kprintf("B_Ag_target_speed :%d\r\n", (rt_int32_t)B_Ag_target_speed);
//                        rt_kprintf("C_Ag_target_speed :%d\r\n", (rt_int32_t)C_Ag_target_speed);
//                        rt_kprintf("D_Ag_target_speed :%d\r\n", (rt_int32_t)D_Ag_target_speed);
                    }
                    else {
//                        for(int i = 0; i < 4; i++)
//                        {
//                            pid_reset(&MOTOR_Ag[i]);
//                        }
                        A_Ag_target_speed = 0;
                        B_Ag_target_speed = 0;
                        C_Ag_target_speed = 0;
                        D_Ag_target_speed = 0;
                        A_Ag_Target = A_Ag_Measure;
                        B_Ag_Target = B_Ag_Measure;
                        C_Ag_Target = C_Ag_Measure;
                        D_Ag_Target = D_Ag_Measure;
                    }

                    //摄像头抓取环
                    if (K230_catch_pid_enable_flag == 1)
                    {
                        //麦轮y轴方向上的速度PWM(左右)
                        A_K230_target_catch_x = Moter_pid_compute(&MOTOR_k230_catch_x[A], 400, toy_x);//强转类型放到最后合成速度再做
                        B_K230_target_catch_x = Moter_pid_compute(&MOTOR_k230_catch_x[B], 400, toy_x);
                        C_K230_target_catch_x = Moter_pid_compute(&MOTOR_k230_catch_x[C], 400, toy_x);
                        D_K230_target_catch_x = Moter_pid_compute(&MOTOR_k230_catch_x[D], 400, toy_x);
                        //x方向上的速度PWM（前后）
                        A_K230_target_catch_y = Moter_pid_compute(&MOTOR_k230_catch_y[A], 240, toy_y);
                        B_K230_target_catch_y = Moter_pid_compute(&MOTOR_k230_catch_y[B], 240, toy_y);
                        C_K230_target_catch_y = Moter_pid_compute(&MOTOR_k230_catch_y[C], 240, toy_y);
                        D_K230_target_catch_y = Moter_pid_compute(&MOTOR_k230_catch_y[D], 240, toy_y);
                    }
                    else {
//                        for(int i = 0; i < 4; i++)
//                        {
//                            pid_reset(&MOTOR_k230_catch_x[i]);
//                            pid_reset(&MOTOR_k230_catch_y[i]);
//                        }
                        //y方向上的速度PWM
                        A_K230_target_catch_x = 0;
                        B_K230_target_catch_x = 0;
                        C_K230_target_catch_x = 0;
                        D_K230_target_catch_x = 0;
                        //x方向上的速度PWM
                        A_K230_target_catch_y = 0;
                        B_K230_target_catch_y = 0;
                        C_K230_target_catch_y = 0;
                        D_K230_target_catch_y = 0;
                    }

                    if(K230_position_pid_enable_flag == 1)
                    {
                        //距离环算出麦轮x轴方向上的速度PWM
                        A_K230_target_distance = Moter_pid_compute(&MOTOR_k230_d[A], 25, toy_distance);//强转类型放到最后合成速度再做
                        B_K230_target_distance = Moter_pid_compute(&MOTOR_k230_d[B], 25, toy_distance);
                        C_K230_target_distance = Moter_pid_compute(&MOTOR_k230_d[C], 25, toy_distance);
                        D_K230_target_distance = Moter_pid_compute(&MOTOR_k230_d[D], 25, toy_distance);
                        //摄像头画面x的位置环计算出Y方向上的速度PWM
                        A_K230_target_xx = Moter_pid_compute(&MOTOR_k230_x[A], 400, toy_x);
                        B_K230_target_xx = Moter_pid_compute(&MOTOR_k230_x[B], 400, toy_x);
                        C_K230_target_xx = Moter_pid_compute(&MOTOR_k230_x[C], 400, toy_x);
                        D_K230_target_xx = Moter_pid_compute(&MOTOR_k230_x[D], 400, toy_x);
                    }
                    else {
//                        for(int i = 0; i < 4; i++)
//                        {
//                            pid_reset(&MOTOR_k230_d[i]);
//                            pid_reset(&MOTOR_k230_x[i]);
//                        }
                        //距离环算出x方向上的速度PWM
                        A_K230_target_distance = 0;
                        B_K230_target_distance = 0;
                        C_K230_target_distance = 0;
                        D_K230_target_distance = 0;
                        //x的位置环计算出y方向上的速度PWM
                        A_K230_target_xx = 0;
                        B_K230_target_xx = 0;
                        C_K230_target_xx = 0;
                        D_K230_target_xx = 0;
                    }

                    // 速度目标合成（基础速度 + 角度修正 + 位置修正）基础速度不给时动不起来的（留给普通四轮小车使用），基准速度单纯是因为要巡线才加的
//                    A_Ve_Target = base_speed + A_po_target_speed + A_Ag_target_speed + A_K230_target_speed;
//                    B_Ve_Target = base_speed + B_po_target_speed + B_Ag_target_speed + B_K230_target_speed;
//                    C_Ve_Target = base_speed + C_po_target_speed + C_Ag_target_speed + C_K230_target_speed;
//                    D_Ve_Target = base_speed + D_po_target_speed + D_Ag_target_speed + D_K230_target_speed;

                    // 速度目标合成，麦轮小车使用，考虑要不要加基准速度
                    //x为前后，+为前 | y为左右，+为右 | z为顺逆，+为顺，(Axle_spacing+Wheel_spacing)已经是一半了
                    //A左上B左下C右下D右上
                    /*各个环的电机速度的PWM合成，后面的MOTOR_ve[A].Target已经是三向合成过的了*/

                    A_Ve_Target = (A_K230_target_distance + A_K230_target_xx + A_Ag_target_speed * Car_HW) + (A_K230_target_catch_y + A_K230_target_catch_x) + PWM_cirle + base_speed;//左前
                    B_Ve_Target = (B_K230_target_distance - B_K230_target_xx + B_Ag_target_speed * Car_HW) + (B_K230_target_catch_y - B_K230_target_catch_x) + PWM_cirle + base_speed;//左后
                    C_Ve_Target = (C_K230_target_distance + C_K230_target_xx - C_Ag_target_speed * Car_HW) + (C_K230_target_catch_y + C_K230_target_catch_x) - PWM_cirle + base_speed;//右后
                    D_Ve_Target = (D_K230_target_distance - D_K230_target_xx - D_Ag_target_speed * Car_HW) + (D_K230_target_catch_y - D_K230_target_catch_x) - PWM_cirle + base_speed;//右前
//                    rt_kprintf("A : %d \r\n", (rt_int16_t)A_Ve_Target);
//                    rt_kprintf("B : %d \r\n", (rt_int16_t)B_Ve_Target);
//                    rt_kprintf("C : %d \r\n", (rt_int16_t)C_Ve_Target);
//                    rt_kprintf("D : %d \r\n", (rt_int16_t)D_Ve_Target);
//                    Drive_motor((rt_int32_t)A_Ve_Target, (rt_int32_t)B_Ve_Target, (rt_int32_t)C_Ve_Target, (rt_int32_t)D_Ve_Target);//给四个电机赋值PWM
                case APP_PID_STOP:
                    break;
                default:
                    break;
            }
     }
}

void Circle_Move(rt_int32_t speed)
{
    PWM_cirle = speed;
}

void print_rt(void)
{
//    rt_kprintf("A : %d \r\n", (rt_int32_t)A_Ve_Target);//打印速度的值使用最大的类型rt_int32_t
//    rt_kprintf("B : %d \r\n", (rt_int32_t)B_Ve_Target);
//    rt_kprintf("C : %d \r\n", (rt_int32_t)C_Ve_Target);
//    rt_kprintf("D : %d \r\n", (rt_int32_t)D_Ve_Target);
//    rt_kprintf("K230_position_pid_enable_flag : %d\r\n", K230_position_pid_enable_flag);
//    rt_kprintf("A_K230_target_distance %d\r\n", (rt_int32_t)A_K230_target_xx);
//    rt_kprintf("B_K230_target_distance %d\r\n", (rt_int32_t)B_K230_target_xx);
//    rt_kprintf("C_K230_target_distance %d\r\n", (rt_int32_t)C_K230_target_xx);
//    rt_kprintf("D_K230_target_distance %d\r\n", (rt_int32_t)D_K230_target_xx);
    rt_kprintf("[EncoderTimer] Tick: %u | Encoder A: %d B: %d C: %d D: %d\n",
               rt_tick_get(),
               -(rt_int32_t)A_encoder.Encoder,
               -(rt_int32_t)B_encoder.Encoder,
               (rt_int32_t)C_encoder.Encoder,
               (rt_int32_t)D_encoder.Encoder);
}


/*************************************两轮***************************************/
//// 位置式前后走
////实现原理：
////​电机1：目标位置 ​正向增加 → 电机正转（假设编码器正向计数为前进方向）。
////​电机2：目标位置 ​反向减少 → 电机反转（若电机对称安装，则两轮同速反向 → 小车直线前进）。
////​差速关系：两电机目标位置变化量相等但方向相反，确保两轮速度对称。
//void car_go_position(int32_t _position_target) {
//    motor1_position_target += _position_target;
//    motor2_position_target -= _position_target;
//}
//// 位置式旋转
////​作用：控制小车绕中心原地旋转指定角度（通过位置差实现）。
////| 电机编号      | 编码器正方向 | 意义     |
////| --------- | ------ | ------ |
////| 左轮 motor1 | 正方向前进  | 左轮向前是正 |
////| 右轮 motor2 | 正方向后退  | 右轮向前是负 |
////_position_target = 500
////motor1_position_target += -500   // 左轮后退
////motor2_position_target -= 500    // 右轮前进（因为电机方向反）
////原地左转
//void car_spin_position(int32_t _position_target)
//{
//    motor1_position_target += -_position_target;
//    motor2_position_target -= _position_target;
//}
//
///*因为motor1_position_target不能被外部调用，所以提供一个接口函数*/
//int64_t get_m1_position_target(void) { return motor1_position_target; } // 返回电机1的目标位置
//int64_t get_m1_position_measurement(void) { return position1_measurement; } // 返回电机1的实际位置（编码器值 - 30000）
/****************************************************************************************************/

MSH_CMD_EXPORT(change_distance_60, change_distance_60);
MSH_CMD_EXPORT(change_distance_40, change_distance_40);
MSH_CMD_EXPORT(change_distance_30, change_distance_30);
MSH_CMD_EXPORT(change_distance_50, change_distance_50);
MSH_CMD_EXPORT(change_distance_20, change_distance_20);
MSH_CMD_EXPORT(change_x_600, change_x_600);
MSH_CMD_EXPORT(change_x_400, change_x_400);
MSH_CMD_EXPORT(change_x_300, change_x_300);
MSH_CMD_EXPORT(change_x_500, change_x_500);
MSH_CMD_EXPORT(print_rt, print_rt);
MSH_CMD_EXPORT(active_K230_position_pid, active_K230_position_pid);
MSH_CMD_EXPORT(deactive_K230_position_pid, deactive_K230_position_pid);
MSH_CMD_EXPORT(A_50_Ve_Target, A_50_Ve_Target);
MSH_CMD_EXPORT(A_60_Ve_Target, A_60_Ve_Target);
MSH_CMD_EXPORT(A_40_Ve_Target, A_40_Ve_Target);
MSH_CMD_EXPORT(A_70_Ve_Target, A_70_Ve_Target);
MSH_CMD_EXPORT(A_80_Ve_Target, A_80_Ve_Target);
